#include <stdint.h>
#include <string>
#include <memory.h>

#define H264_ID        0x1b
#define H265_ID        0x24
#define MPEG_ID        0x10
#define SVACV_ID       0x80

#define G711_ID        0x90
#define SVACA_ID       0x9b

/* 长度14字节 */
const uint8_t PS_HEAD[] =
{
    /*PS头*/
    0x00, 0x00, 0x01, 0xba,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,        /*时间戳*/
    0x01, 0x47, 0xb3,
    0xf8
};

const uint8_t SYS_MAP_HEAD[] =
{
    /*PS_SYS头*/
    0x00, 0x00, 0x01, 0xbb,
    0x00, 0x0c,                                 /*sys头长度,不含自己，6+3*流的数目*/
    0x80, 0xa3, 0xd9,                           /*速率*/
    0x04, 0xe1,                                 /*音频流数，视频流数加3个1标识*/
    0xff,                                       /**/
    0xb9, 0xe0, 0x00, 0xb8, 0xc0, 0x40,         /*流信息，b9视频，b8音频*/
    /*PS_MAP头*/
    0x00, 0x00, 0x01, 0xbc,
    0x00, 0x12,                                 /*psm长度*/
    0x04, 0xff,                                 /**/
    0x00, 0x00, 0x00, 0x08,                     /*固定2路流*/
    0x1b, 0xe0, 0x00, 0x00,                     /*视频，第一个字节(0x1b), 更具不同的视频编码改变即可封装不同的流，见开头宏定义*/
    0x90, 0xc0, 0x00, 0x00,                     /*音频，同视频*/
    0x00, 0x00, 0x00, 0x00                      /*4b CRC，暂时没设置*/
};

/* 长度19字节 */
const uint8_t PES_HEAD[] =
{
    /*PS_PES头*/
    0x00, 0x00, 0x01, 0xe0,
    0x00, 0x00,                                 /*pes长度*/
    0x80, 0xc0,                                 /*附加信息*/
    0x0a,                                       /*附加信息长度*/
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00      /*pts和pds*/
};

void SetHeaderTimeStamp(uint8_t *dest, uint64_t pts)
{
    uint8_t *scr_buf = dest + 4;
    scr_buf[0] = 0x40 | (((uint8_t)(pts >> 30) & 0x07) << 3) | 0x04 | ((uint8_t)(pts >> 28) & 0x03);
    scr_buf[1] = (uint8_t)((pts >> 20) & 0xff);
    scr_buf[2] = (((uint8_t)(pts >> 15) & 0x1f) << 3) | 0x04 | ((uint8_t)(pts >> 13) & 0x03);
    scr_buf[3] = (uint8_t)((pts >> 5) & 0xff);
    scr_buf[4] = (((uint8_t)pts & 0x1f) << 3) | 0x04;
    scr_buf[5] = 1;
}

// 设置PES头中的PTS和DTS字段
void SetPESTimeStamp(uint8_t *buff, uint64_t ts)
{
    buff += 9;
    // PTS
    buff[0] = (uint8_t)(((ts >> 30) & 0x07) << 1) | 0x30 | 0x01;
    buff[1] = (uint8_t)((ts >> 22) & 0xff);
    buff[2] = (uint8_t)(((ts >> 15) & 0xff) << 1) | 0x01;
    buff[3] = (uint8_t)((ts >> 7) & 0xff);
    buff[4] = (uint8_t)((ts & 0xff) << 1) | 0x01;
    // DTS
    buff[5] = (uint8_t)(((ts >> 30) & 0x07) << 1) | 0x10 | 0x01;
    buff[6] = (uint8_t)((ts >> 22) & 0xff);
    buff[7] = (uint8_t)(((ts >> 15) & 0xff) << 1) | 0x01;
    buff[8] = (uint8_t)((ts >> 7) & 0xff);
    buff[9] = (uint8_t)((ts & 0xff) << 1) | 0x01;
}

int GetSinglePESHeader(uint8_t *header, uint64_t mtime, uint16_t farmLen)
{
    farmLen += 13;
    memcpy(header, PES_HEAD, sizeof(PES_HEAD));

    /* 可以看到, pes 头的第4和第5个字节为 pes 长度 */
    /* 使用两个字节记录长度信息, 因此最大长度不能超过 65535 */
    /* 而这个 65535 又是 pes 头 + es 码流合起来的长度 */
    *(header + 4) = (uint8_t)(farmLen >> 8);
    *(header + 5) = (uint8_t)farmLen;

    SetPESTimeStamp(header, mtime);
    return sizeof(PES_HEAD);
}

int GetPSHeader(uint8_t *header, uint64_t mtime, uint16_t farmLen, int streamType, int farmType)
{
    /* 音频帧, 不做ps头 */
    if(streamType == 2)        //语音包
    {
        GetSinglePESHeader(header, mtime, farmLen);
        *(header + 3) = 0xc0;
        return sizeof(PES_HEAD);
    }
    /* I帧除了ps头以外还要有sys_map */
    else if(farmType == 1)    //I帧
    {
        memcpy(header, PS_HEAD, sizeof(PS_HEAD));
        SetHeaderTimeStamp(header, mtime);
        header += sizeof(PS_HEAD);

        memcpy(header, SYS_MAP_HEAD, sizeof(SYS_MAP_HEAD));
        header += sizeof(SYS_MAP_HEAD);

        GetSinglePESHeader(header, mtime, farmLen);
        return sizeof(PS_HEAD) + sizeof(SYS_MAP_HEAD) + sizeof(PES_HEAD);
    }
    else
    {
        memcpy(header, PS_HEAD, sizeof(PS_HEAD));
        SetHeaderTimeStamp(header, mtime);
        header += sizeof(PS_HEAD);

        GetSinglePESHeader(header, mtime, farmLen);
        return sizeof(PS_HEAD) + sizeof(PES_HEAD);
    }
}

/* 封装结束后的PS帧缓冲区 */
unsigned char PSFrameBuffer[10 * 1024 * 1024];

/**
 * brief
 * param pFrame 输入原始es码流
 * param nFrameLength 输入原始es码流长度
 * param nIFrameFlag 是否是I帧(对于视频)
 * param nStreamType 输入es码流类型, 264/265/aac/g711a/etc...
 * param nTimeStamp 码流时间戳
 */
int TransPSFrame(char *pFrame, int nFrameLength, int nIFrameFlag, int nStreamType, u_int nTimeStamp)
{
    if(!pFrame || !nFrameLength)
        return 0;

    /* 每个pes最多65400数据 */
    int PesLenth = nFrameLength > 65400 ? 65400 : nFrameLength;

    /* 第一个pes需要有ps头，其它不需要，音频直接打包pes（00 00 01 c0) */
    /* 将 pes 和 ps 头信息填写到输入 buffer 位置, 通过返回值返回 ps + pes 头信息长度 */
    int psHeadLen = GetPSHeader(PSFrameBuffer, nTimeStamp, PesLenth, nStreamType, nIFrameFlag);

    /* 在ps头位置后面直接追加es码流数据 */
    memcpy(PSFrameBuffer + psHeadLen, pFrame, PesLenth);

    int psSize = psHeadLen + PesLenth;          // ps输出数据中: 目前已经填充 ps 封装好的数据的长度
    int pod = PesLenth;                         // es输入数据中: 目前已经被封装到 ps 码流中的数据长度

    /* 如果说 nFrameLength 小于等于 65400, 则下面表达式相减为0 */
    /* 如果说 nFrameLength 大于 65400, 说明ps码流数据超过了单个PES长度限制 */
    nFrameLength -= PesLenth;
    while (nFrameLength > 0)
    {
        /* 同样的道理, 需要不断的判断剩余的pes码流是否超出pes长度限制 */
        PesLenth = nFrameLength > 65400 ? 65400 : nFrameLength;
        psHeadLen = GetSinglePESHeader(PSFrameBuffer + psSize, nTimeStamp, PesLenth);

        /* 再前一段pes包, 再加上当前的pes头部后面, 继续追加es码流 */
        memcpy(PSFrameBuffer + psSize + psHeadLen, pFrame + pod, PesLenth);
        psSize += (PesLenth + psHeadLen);
        pod += PesLenth;
        nFrameLength -= PesLenth;
    }

#if 0
    /* 写入本地文件 */
    static FILE *fp = fopen("my.ps", "wb");
    if(fp)
        fwrite(PSFrameBuffer, 1, psSize, fp);
#endif

    return psSize;
}